    //------------------------------------------------
    // Communication with Pololu Micro Maestro Servo Controller
    //------------------------------------------------
    // http://www.pololu.com/catalog/product/1350
    // Documentation : http://www.pololu.com/docs/0J40
    // based on the Pololu micro serial servo controller by mike crist 2009-01-03
    //
    // Xevel, 2010-03-01
    // http://xevel.fr

    //----------------------------------------------------------------------
    //                           Serial settings           
    //----------------------------------------------------------------------

    // GENERAL SETTINGS
    //#define MAESTRO_DETECT_BAUDRATE    // comment if using "UART, fixed baud rate"
#define MAESTRO_BAUDRATE         9600  // set to the baudrate used.
#define MAESTRO_USE_RST_PIN  // uncomment if you want to be able to reset with the RST pin of the board
#define servoControllerResetPin  16  // digital pin connected to Maestro RST
   
    //---------------------------------------------------------------------
    // HARDWARE SERIAL ONLY. Comment this block if using a software serial    
      #define MaestroSerial Serial3 // set to the name of the serial you want to use
      #define MAESTRO_START_TRANSMISSION()
      #define MAESTRO_END_TRANSMISSION()   
    //---------------------------------------------------------------------

#define NoNl     false
#define Nl       true

byte servo=0;

char ch;

int aktPosAzimuth=2000;
int aktPosElv=2000;

    void setup() {
      Serial.begin(9600);  
      maestro_init_serial();
      
    }
    
void loop() {
     
     move_random();
     delay(5000); 
     static int v = 0;
     static byte low;
     static byte high;
     static int servo_position;
     
     
     if (Serial.available()) { 
          ch = Serial.read();  
           switch(ch) {
             case '0'...'9':
                v = v * 10 + ch - '0';
                break;
             case 'c':
                show("choose Servo",v,Nl);
                servo=v;
                v=0;
                break;
             case 'e':
                show("test e:",v,Nl);                               
                v = 0; 
                break;
             case 'g':
                servo_position=maestro_get_position(0);                ;
                show("position:",servo_position,Nl);
                break;
             case 'h':
                high=v;
                show("low:",low,Nl);
                show("high:",high,Nl);
                v=0;
                break;
             case 'l':
                low=v;
                show("low:",low,Nl);
                show("high:",high,Nl);
                v =0;
                break;
             case  'k':
                maestro_set_targetlowHigh(servo,low,high);
                break;
             case 'm':
                show("set servo 0 minissc;angle",v,Nl);                                                
                maestro_set_target_minissc(servo, v);
                v=0;
                break; 
             case 'o':
                show("set servo off",v,Nl); 
                maestro_set_servo_off(servo);
                break;
             case 'r':
                show("maestor reset",0,Nl);
                maestro_hard_reset();
                v=0;
                break;
             case 's':
                show("set servo:angle",v,Nl);
                show("servo No:",servo,Nl);
                int angle=v;
                maestro_set_target(servo, angle);                 
                v=0;
                break; 
            }
      }  
      
}
     
    // Servo commands
    void maestro_set_speed(byte servo, int speedVal);
    void maestro_set_speed(byte device, byte servo, int speedVal);
    void maestro_set_acceleration(byte servo, byte accel);
    void maestro_set_acceleration(byte device, byte servo, byte accel);
    void maestro_go_home();
    void maestro_go_home(byte device);
    void maestro_set_servo_off(byte servo);
    void maestro_set_servo_off(byte device, byte servo);
    void maestro_set_all_servos_off();
    //int maestro_get_position(byte servo);
    int maestro_get_position(byte device, byte servo);
    byte maestro_get_moving_state();
    byte maestro_get_moving_state(byte device);
    int maestro_get_errors();
    int maestro_get_errors(byte device);

    // Script commands
    void maestro_stop_script();
    void maestro_stop_script(byte device);
    byte maestro_get_script_running();
    byte maestro_get_script_running(byte device);
    void maestro_restart_script_param(byte subroutine, int param);
    void maestro_restart_script_param(byte device, byte subroutine, int param);
    void maestro_restart_script(byte subroutine);
    void maestro_restart_script(byte device, byte subroutine);

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------

    void show(char nameStr[], double val, boolean newline) {
      Serial.print(nameStr);  
      Serial.print("=");
      if (newline)
       Serial.println(val);
      else Serial.print(val);
    }

    void move_random() {
        int randomDist=800;
        aktPosAzimuth=constrain(aktPosAzimuth+random(0, randomDist)-randomDist/2,0,4000);
        aktPosElv=constrain(aktPosElv+random(0, randomDist)-randomDist/2,2000,4000);
        moveToAktPos();
    }
    
    void moveToAktPos() {
        maestro_set_target(0, aktPosAzimuth); 
        maestro_set_target(1,aktPosElv);
    }
    
    void maestro_init_serial(){
    #ifdef MAESTRO_USE_RST_PIN
      pinMode(servoControllerResetPin,OUTPUT);
      maestro_hard_reset(); // reset the servo controller before speaking to it
    #endif

      MaestroSerial.begin(MAESTRO_BAUDRATE); // init the serial communication to the board
      delay(5); // give a little time to the Maestro to breeze
     
    #ifdef MAESTRO_DETECT_BAUDRATE
      MaestroSerial.print(0xAA, BYTE);    //start byte to init baudrate
    #endif
    }



    void maestro_send_header(byte cmd, byte device){
      // sends the header of the command, depending ont the protocol
     
      if ( (cmd & 0x80) == 0 ) {   // Pololu protocol
        MaestroSerial.print(0xAA, BYTE);    //start byte
        MaestroSerial.print(device, BYTE);  //device
      }
      MaestroSerial.print(cmd, BYTE);
    }

    void maestro_send_command(byte cmd, byte device){
      // sends a command without data
     
      MAESTRO_START_TRANSMISSION();
      maestro_send_header(cmd, device);
      MAESTRO_END_TRANSMISSION();
    }

    void maestro_send_command(byte cmd, byte val1, byte device){
      // sends a command with 1 byte of data

      MAESTRO_START_TRANSMISSION();
      maestro_send_header(cmd, device);
      MaestroSerial.print(val1, BYTE);
      MAESTRO_END_TRANSMISSION();
    }

    void maestro_send_command(byte cmd, byte val1, int val2, byte device){
      // sends a command with 3 byte of data

      MAESTRO_START_TRANSMISSION();
      maestro_send_header(cmd, device);
      MaestroSerial.print(val1, BYTE);
      MaestroSerial.print(val2 & 0x7f, BYTE); //data lower 7 bits
      MaestroSerial.print((val2 >> 7) & 0x7f, BYTE); //data bits 7-13
      MAESTRO_END_TRANSMISSION();
    }


    int maestro_receive_int(){
      int low, high;
     
      while(!MaestroSerial.available()); // wait until some data is present
     
      high = MaestroSerial.read();       // read the first byte
      while(MaestroSerial.available()){  // while the rx buffer is not empty
        low=high;                        // put the previous value read in "low"
        high = MaestroSerial.read();     // put the last value in "high"
      }
      // at this point, "high" contains the last value received, and "low" the one
      // before that. Any garbage that could have been in the rx buffer has been
      // effectively thrown away.
     
      return low | (high << 8);
    }

    byte maestro_receive_byte(){
      byte data;
     
      while(!MaestroSerial.available()); // wait until some data is present
     
      while(MaestroSerial.available()){  // while the rx buffer is not empty
        data = MaestroSerial.read();     // put the last value in "data"
      }
      // at this point, "data" contains the last value received. Any garbage
      // that could have been in the rx buffer has been effectively thrown away.
     
      return data;
    }


    void maestro_hard_reset(){
    #ifdef MAESTRO_USE_RST_PIN
      digitalWrite(servoControllerResetPin, LOW);
      delay(50);
      digitalWrite(servoControllerResetPin, HIGH);
    #endif
    }

    //------------------------------------------------------------
    //                     Servo commands
    //------------------------------------------------------------
    /*void maestro_set_target(byte servo, int angle){
      //set the target for a servo
      //servo is the servo number (typically 0-5)
      //angle is the target, from 256 to 13120, in quarter-microseconds
     
      maestro_send_command(0x84, servo, angle, 0);
    }*/
    
    
    void maestro_set_target(byte servo, int angle){
      
      //This way fine grain angle-Values between 0 und 4032 are possible
      byte lowbyte=(angle+3968)&127;      
      byte highbyte=(angle+3968)>>7;
      
      /* with 'regular' values 992 until 2000
      byte lowbyte=(angle*4)&127;      
      byte highbyte=angle>>5;
      */  
      
      MaestroSerial.print(0x84, BYTE);    //start byte
      MaestroSerial.print(servo, BYTE);   //servo number + device offset
      MaestroSerial.print(lowbyte, BYTE); 
      MaestroSerial.print(highbyte, BYTE);  
      
           
    }
      
    
    void maestro_set_targetlowHigh(byte servo, byte low, byte high){
      //set the target for a servo on a particular device
      //device is the id of the Maestro device (default: 12)
      //servo is the servo number (typically 0-5)
      //angle is the target, from 256 to 13120, in quarter-microseconds      
      MAESTRO_START_TRANSMISSION();
      MaestroSerial.print(0x84, BYTE);    //start byte
      MaestroSerial.print(servo, BYTE);   //servo number + device offset
      MaestroSerial.print(low, BYTE); 
      MaestroSerial.print(high, BYTE);  
      MAESTRO_END_TRANSMISSION();  
           
    }
    
    void maestro_set_target(byte device, byte servo, int angle){
      //set the target for a servo on a particular device
      //device is the id of the Maestro device (default: 12)
      //servo is the servo number (typically 0-5)
      //angle is the target, from 256 to 13120, in quarter-microseconds         
    }

    void maestro_set_target_minissc(byte servo, byte angle){
      // set the target using the Mini-SSC protocol
      //servo is the sum of the servo number (typically 0-5) and the servo offset of the device
      //angle is the target, from 0 to 254, mapped between neutral-range and neutral+range. 127 is neutral.

      //Send a Mini-SSC command
      MAESTRO_START_TRANSMISSION();
      MaestroSerial.print(0xFF, BYTE);    //start byte
      MaestroSerial.print(servo, BYTE);   //servo number + device offset
      MaestroSerial.print(angle, BYTE);   //8-bit target
      MAESTRO_END_TRANSMISSION();
    }


    void maestro_set_speed(byte servo, int speedVal){
      //set the speed for a servo
      //servo is the servo number (typically 0-5)
      //speedVal is servo speed (0=full, 1=slower), in (0.25 μs)/(10 ms)
     
      maestro_send_command(0x87, servo, speedVal, 0);
    }

    void maestro_set_speed(byte device, byte servo, int speedVal){
      //set the speed for a servo on a particular device
      //device is the id of the Maestro device (default: 12)
      //servo is the servo number (typically 0-5)
      //speedVal is servo speed (0=full, 1=slower), in (0.25 μs)/(10 ms)
     
      maestro_send_command(0x07, servo, speedVal, device);
    }


    void maestro_set_acceleration(byte servo, byte accel){
      //set the acceleration for a servo
      //servo is the servo number (typically 0-5)
      //accel is servo acceleration (0=full, 1=slower), in (0.25 μs)/(10 ms)/(80 ms)
     
      maestro_send_command(0x89, servo, accel, 0);
    }

    void maestro_set_acceleration(byte device, byte servo, byte accel){
      //set the acceleration for a servo on a particular device
      //device is the id of the Maestro device (default: 12)
      //servo is the servo number (typically 0-5)
      //accel is servo acceleration (0=full, 1=slower), in (0.25 μs)/(10 ms)/(80 ms)
     
      maestro_send_command(0x09, servo, accel, device);
    }


    void maestro_go_home(){
      //set all servos to their home position. Servos set to "ignore" will remain unchanged.
     
      maestro_send_command(0xA2, 0);
    }

    void maestro_go_home(byte device){
      //set all servos to their home position on a particular device. Servos set to "ignore" will remain unchanged.
      //device is the id of the Maestro device (default: 12)
      //servo is the servo number (typically 0-5)
     
      maestro_send_command(0x22, device);
    }


    void maestro_set_servo_off(byte servo){
      //stop a servo (and leave it limp)
      //servo is the servo number (typically 0-5)

      // To set off a servo, set its target to 0.
      maestro_set_target(servo, 0);
    }

    void maestro_set_servo_off(byte device, byte servo){
      //stop a servo (and leave it limp) on a particular device
      //device is the id of the Maestro device (default: 12)
      //servo is the servo number (typically 0-5)

      // To set off a servo, set its target to 0.
      maestro_set_target(device, servo, 0);
    }

    void maestro_set_all_servos_off(){
      for(int i = 0; i<6; i++){
        maestro_set_servo_off(i);
      }
    }

    int maestro_get_position(byte servo){
      // gets the current position of a servo
      int last_receive_int=1;
      int receive_int=2;
      int tries=3;
      while (last_receive_int!=receive_int && tries-->0) {
        last_receive_int=receive_int;
        maestro_send_command(0x90, servo, 0);
        receive_int=maestro_receive_int();
      }
      
      return receive_int;
    }

    int maestro_get_position(byte device, byte servo){
      // gets the current position of a servo on a particular device
      //device is the id of the Maestro device (default: 12)
     
      maestro_send_command(0x10, servo, device);
      return maestro_receive_int();
    }

    byte maestro_get_moving_state(){
      // checks if some servos are moving on a particular device
     
      maestro_send_command(0x93, 0);
      return maestro_receive_byte();
    }

    byte maestro_get_moving_state(byte device){
      // checks if some servos are moving on a particular device
      //device is the id of the Maestro device (default: 12)
     
      maestro_send_command(0x13, device);
      return maestro_receive_byte();
    }

    int maestro_get_errors(){
      // gets the current error flags
      //device is the id of the Maestro device (default: 12)
     
      maestro_send_command(0xA1, 0);
      return maestro_receive_int();
    }

    int maestro_get_errors(byte device){
      // gets the current error flags on a particular device
      //device is the id of the Maestro device (default: 12)
     
      maestro_send_command(0x21, device);
      return maestro_receive_int();
    }


    //------------------------------------------------------------
    //                     Script commands
    //------------------------------------------------------------

    void maestro_stop_script(){
      //stop the script, if it is currently running

      maestro_send_command(0xA4, 0);
    }

    void maestro_stop_script(byte device){
      //stop the script, if it is currently running
      //device is the id of the Maestro device (default: 12)

      maestro_send_command(0x24, device);
    }


    void maestro_restart_script(byte subroutine){
      //start a script
      //subroutine is the point where the script will start.
     
      maestro_send_command(0xA7, subroutine, 0);
    }


    void maestro_restart_script(byte device, byte subroutine){
      //start a script on a particular device
      //device is the id of the Maestro device (default: 12)
      //subroutine is the point where the script will start.
     
      maestro_send_command(0x27, subroutine, device);
    }

    void maestro_restart_script_param(byte subroutine, int param){
      //start a script with a parameter
      //subroutine is the point where the script will start.
      //param is the argument (between 0 and 16383)
     
      maestro_send_command(0xA8, subroutine, param, 0);
    }


    void maestro_restart_script_param(byte device, byte subroutine, int param){
      //start a script with a parameter on a particular device
      //device is the id of the Maestro device (default: 12)
      //subroutine is the point where the script will start.
      //param is the argument (between 0 and 16383)
     
      maestro_send_command(0x28, subroutine, param, device);
    }


    byte maestro_get_script_running(){
      // checks if a script is running
     
      maestro_send_command(0xAE, 0);
      return maestro_receive_byte();
    }

    byte maestro_get_script_running(byte device){
      // checks if a script is running on a particular device
      //device is the id of the Maestro device (default: 12)
     
      maestro_send_command(0x2E, device);
      return maestro_receive_byte();
    }


